//
// Created by d on 11/10/18.
//

#include "cpuTracer.h"
#include <iostream>
#include <glog/logging.h>
#include <map>

using namespace std;

Point::Point(float x, float y, float z) {
    p[0] = x;
    p[1] = y;
    p[2] = z;
}

Point::Point(const Vector3f & pp) {
    p = pp;
}

Point::Point(const Point &pp) {
    this->p = pp.p;
}

Point::Point() {
    p[0] = 0;
    p[1] = 0;
    p[2] = 0;
}

Ray::Ray(const Point origin,const Point p) {
    direction = p.p - origin.p;
    direction.normalize();
}

Ray::Ray(const Point origin, const Vector3f d) {
    o = origin;
    direction = d;
    direction.normalize();
}

Vector3f Ray::operator()(float t) {
    return o.p + t * direction;
}




int Sphere::in(const Point &p) {
    double x = (p.p - center.p).norm();
    if(x < r){
        return 2;
    } else if (x > r){
        return 0;
    } else {
        return 1;
    }
}

float Sphere::firstIntersection(const Ray &ray) {
    float B = 2 * ray.direction.dot(ray.o.p - center.p);
    float A = ray.direction.dot(ray.direction);
    float C = (ray.o.p - center.p).dot(ray.o.p - center.p) - r * r;
    float B2_4AC = B * B - 4 * A * C;
    if(B2_4AC <= 0){
        return -1;
    }
    float t1 = (-B + sqrt(B2_4AC)) / (2*A);
    float t2 = (-B - sqrt(B2_4AC)) / (2*A);
    if(t1 < t2) {
        float tmp = t1;
        t1 = t2;
        t2 = tmp;
    }
    // not t1 > t2
    if(t2 <= 0){
        return t1;
    }
    return t2;

}

Sphere::Sphere(Point c, float r) {
    center = c;
    this->r = r;
}

void pushVertex(vector<int>&index, int a, int b, int c){
    index.push_back(a);
    index.push_back(b);
    index.push_back(c);
}

pair<vector<ShaderElement>, vector<int>> Object::triangulate() {
    vector<ShaderElement> vertex;
    LOG(INFO) << "triangulate";
    auto vertexAndIndex = _triangulate();
    vector<Vector3f> points = vertexAndIndex.first;
    // make the return variable;
    for (auto &point : points) {
        ShaderElement ele{};
        ele.positionX.f = point[0];
        ele.positionY.f = point[1];
        ele.positionZ.f = point[2];
        vertex.push_back(ele);
    }
    return pair<vector<ShaderElement>, vector<int>>(
            vertex, vertexAndIndex.second);
}


pair<vector<Vector3f>, vector<int> > Sphere::_triangulate(){
    LOG(INFO) << "triangulate Sphere";
    vector<int> index;

    // init shape is the Tetrahedron
    vector<Vector3f> points;
    points = (vector<Vector3f>)VectorCreator<Vector3f>
                (Vector3f(0, 0, 1))
                (Vector3f(-1,0, 0))
                (Vector3f(0,-1, 0))
                (Vector3f(1, 0, 0))
                (Vector3f(0, 1, 0))
                (Vector3f(0, 0,-1));
    pushVertex(index, 0, 1, 2);
    pushVertex(index, 0, 2, 3);
    pushVertex(index, 0, 3, 4);
    pushVertex(index, 0, 4, 1);
    pushVertex(index, 5, 1, 2);
    pushVertex(index, 5, 2, 3);
    pushVertex(index, 5, 3, 4);
    pushVertex(index, 5, 4, 1);
    // for(int i = 0; i < sizeof(init_arr) / sizeof(int); i+=1){
    //     index.push_back(init_arr[i]);
    // }
    // add more triangle here
    //         a
    //        /\
    //       /  \
    //     d/____\f
    //     /\    /\
    //    /  \  /  \
    //   /____\/____\
    //  b      e     c
    for(int i = 0; i < 4; i++){
        vector<int> new_index;
        // generate new vertexes
        map<pair<int, int>, int> centerPointMap;
        auto add_vertex = [&] (int a, int b)  {
            if(centerPointMap.find(pair<int, int>(a, b)) == centerPointMap.end()){
                Vector3f newPoint = points[a]+points[b];
                newPoint.normalize();
                points.push_back(newPoint);
                centerPointMap[pair<int, int>(a,b)] = static_cast<int>(points.size() - 1);
                // since the visiting order is the same, the following operation is not needed.
                // centerPointMap[pair<int, int>(b,a)] = static_cast<int>(points.size() - 1);
            }
        };
        for(int j = 0; j < index.size(); j+=3) {
            int i_a, i_b, i_c;
            i_a = index[j];
            i_b = index[j + 1];
            i_c = index[j + 2];
            add_vertex(i_a,i_b);
            add_vertex(i_b, i_c);
            add_vertex(i_a, i_c);
        }
        for(int j = 0; j < index.size(); j+=3){
            Vector3f a, b, c, d, e, f;
            int i_a, i_b, i_c, i_d, i_e, i_f;
            i_a = index[j];
            i_b = index[j + 1];
            i_c = index[j + 2];
            i_d = centerPointMap[pair<int, int>(i_a, i_b)];
            i_e = centerPointMap[pair<int, int>(i_b, i_c)];
            i_f = centerPointMap[pair<int, int>(i_a, i_c)];

            a = points[i_a];
            b = points[i_b];
            c = points[i_c];

            pushVertex(new_index, i_a, i_d, i_f);
            pushVertex(new_index, i_d, i_b, i_e);
            pushVertex(new_index, i_d, i_f, i_e);
            pushVertex(new_index
                    , i_c, i_f, i_e);
        }
        index.swap(new_index);
    }
    // post process
    for(auto & vec: points){
        vec = vec * r + center.p;
    }
    return pair<vector<Vector3f>, vector<int> >(points, index);
}


Sphere::Sphere() = default;

int IntersectObject::in(const Point &p) {
    int edge = 2;
    for(auto &obj : objects){
        if(obj->in(p) == 0){
            return 0;
        }
        if(obj->in(p) == 1){
            edge = 1;
        }
    }
    return edge;
}

pair<vector<Vector3f>, vector<int> >
IntersectObject::_triangulate(){
    LOG(INFO) << "triangle IntersectObject";
    vector<Vector3f> points;
    vector<int> indexs;
    for(auto & obj : objects){
        LOG(INFO) << "testing Objec";
        auto triangulateResult = (*obj)._triangulate();
        // vector<Vector3f> points;
        vector<int> index = triangulateResult.second;
        vector<int> pointsIn;
        map<int, int> pointMap;

        auto addOldTriangle = [&](int ii){
            LOG(INFO) << "adding points";
            for (int i = ii; i < ii+3; ++i) {
                if(pointMap.find(index[i]) == pointMap.end()){
                    pointMap[index[i]] = points.size();
                }
                indexs.push_back(pointMap[index[i]]);
                points.push_back(triangulateResult.first[index[i]]);
                LOG(INFO) << points.size();
            }
        };

        for(auto & point: triangulateResult.first){
            pointsIn.push_back(this->in(Point(point)));
            // LOG(INFO) << this->in(Point(point));
        }
        for (int i = 0; i < index.size(); i+=3){
            vector<int> trianglePoints = (vector<int>) VectorCreator<int>
                    (pointsIn[index[i]])
                    (pointsIn[index[i+1]])
                    (pointsIn[index[i+2]]);
            if(anyOf<int>(trianglePoints, [](int a)->bool{return a == 0;})){
                // nothing in, do nothing
                // LOG(INFO) << "out";
                assert(
                    pointsIn[index[i]] == 0 ||
                    pointsIn[index[i+1]] == 0 ||
                    pointsIn[index[i+2]] == 0 
                    );
            } else {
                // add the traingle
                LOG(INFO) << "ADDING triangle";
                addOldTriangle(i);
            } // TODO adding code to generate new triabgles for edges.
        }
    }
    return pair<std::vector<Vector3f>, std::vector<int> >(points, indexs);
}

float IntersectObject::firstIntersection(const Ray &ray) {
    assert(false);
    return 0;
}

void IntersectObject::addObject(Object * object){
    objects.push_back(object);
}